#ifdef __GX__
#include <gccore.h>
#endif // __GX__

#include "Types.h"
#include "GBI.h"

static inline s16 GetResult1( u32 A, u32 B, u32 C, u32 D, u32 E )
{
	s16 x = 0;
	s16 y = 0;
	s16 r = 0;

	if (A == C) x += 1; else if (B == C) y += 1;
	if (A == D) x += 1; else if (B == D) y += 1;
	if (x <= 1) r += 1; 
	if (y <= 1) r -= 1;

	return r;
}

static inline s16 GetResult2( u32 A, u32 B, u32 C, u32 D, u32 E) 
{
	s16 x = 0; 
	s16 y = 0;
	s16 r = 0;

	if (A == C) x += 1; else if (B == C) y += 1;
	if (A == D) x += 1; else if (B == D) y += 1;
	if (x <= 1) r -= 1; 
	if (y <= 1) r += 1;

	return r;
}


static inline s16 GetResult( u32 A, u32 B, u32 C, u32 D )
{
	s16 x = 0; 
	s16 y = 0;
	s16 r = 0;

	if (A == C) x += 1; else if (B == C) y += 1;
	if (A == D) x += 1; else if (B == D) y += 1;
	if (x <= 1) r += 1; 
	if (y <= 1) r -= 1;

	return r;
}

static inline u16 INTERPOLATE4444( u16 A, u16 B)
{
	if (A != B)
		return	((A & 0xEEEE) >> 1) + 
				((B & 0xEEEE) >> 1) |
				(A & B & 0x1111);
	else
		return A;
}

static inline u16 INTERPOLATE5551( u16 A, u16 B)
{
	if (A != B)
		return	((A & 0xF7BC) >> 1) + 
				((B & 0xF7BC) >> 1) |
				(A & B & 0x0843);
	else
		return A;
}

static inline u32 INTERPOLATE8888( u32 A, u32 B)
{
	if (A != B)
		return	((A & 0xFEFEFEFE) >> 1) + 
				((B & 0xFEFEFEFE) >> 1) |
				(A & B & 0x01010101);
	else
		return A;
}

static inline u16 Q_INTERPOLATE4444( u16 A, u16 B, u16 C, u16 D)
{
	u16 x =	((A & 0xCCCC) >> 2) +
				((B & 0xCCCC) >> 2) +
				((C & 0xCCCC) >> 2) +
				((D & 0xCCCC) >> 2);
	u16 y =	(((A & 0x3333) +
				(B & 0x3333) +
				(C & 0x3333) +
				(D & 0x3333)) >> 2) & 0x3333;
	return x | y;
}

static inline u16 Q_INTERPOLATE5551( u16 A, u16 B, u16 C, u16 D)
{
	u16 x =	((A & 0xE738) >> 2) +
				((B & 0xE738) >> 2) +
				((C & 0xE738) >> 2) +
				((D & 0xE738) >> 2);
	u16 y =	(((A & 0x18C6) +
				(B & 0x18C6) +
				(C & 0x18C6) +
				(D & 0x18C6)) >> 2) & 0x18C6;
	u16 z =	((A & 0x0001) +
		        (B & 0x0001) +
				(C & 0x0001) +
				(D & 0x0001)) > 2 ? 1 : 0;
	return x | y | z;
}

static inline u32 Q_INTERPOLATE8888( u32 A, u32 B, u32 C, u32 D)
{
	u32 x =	((A & 0xFCFCFCFC) >> 2) +
				((B & 0xFCFCFCFC) >> 2) +
				((C & 0xFCFCFCFC) >> 2) +
				((D & 0xFCFCFCFC) >> 2);
	u32 y =	(((A & 0x03030303) +
				(B & 0x03030303) +
				(C & 0x03030303) +
				(D & 0x03030303)) >> 2) & 0x03030303;
	return x | y;
}

void _2xSaI4444( u16 *srcPtr, u16 *destPtr, u16 width, u16 height, s32 clampS, s32 clampT )
{
	u16 destWidth = width << 1;
	//u16 destHeight = height << 1;

    u32 colorA, colorB, colorC, colorD,
          colorE, colorF, colorG, colorH,
          colorI, colorJ, colorK, colorL,
          colorM, colorN, colorO, colorP;
	u32 product, product1, product2;

	s16 row0, row1, row2, row3;
	s16 col0, col1, col2, col3;

	for (u16 y = 0; y < height; y++)
	{
		if (y > 0)
			row0 = -width;
		else
			row0 = clampT ? 0 : (height - 1) * width;

		row1 = 0;

		if (y < height - 1)
		{
			row2 = width;

			if (y < height - 2) 
				row3 = width << 1;
			else
				row3 = clampT ? width : -y * width;
		}
		else
		{
			row2 = clampT ? 0 : -y * width;
			row3 = clampT ? 0 : (1 - y) * width;
		}

        for (u16 x = 0; x < width; x++)
        {
			if (x > 0)
				col0 = -1;
			else
				col0 = clampS ? 0 : width - 1;

			col1 = 0;

			if (x < width - 1)
			{
				col2 = 1;

				if (x < width - 2) 
					col3 = 2;
				else
					col3 = clampS ? 1 : -x;
			}
			else
			{
				col2 = clampS ? 0 : -x;
				col3 = clampS ? 0 : 1 - x;
			}

//---------------------------------------
// Map of the pixels:                    I|E F|J
//                                       G|A B|K
//                                       H|C D|L
//                                       M|N O|P
            colorI = *(srcPtr + col0 + row0);
            colorE = *(srcPtr + col1 + row0);
            colorF = *(srcPtr + col2 + row0);
            colorJ = *(srcPtr + col3 + row0);

            colorG = *(srcPtr + col0 + row1);
            colorA = *(srcPtr + col1 + row1);
            colorB = *(srcPtr + col2 + row1);
            colorK = *(srcPtr + col3 + row1);

            colorH = *(srcPtr + col0 + row2);
            colorC = *(srcPtr + col1 + row2);
            colorD = *(srcPtr + col2 + row2);
            colorL = *(srcPtr + col3 + row2);

            colorM = *(srcPtr + col0 + row3);
            colorN = *(srcPtr + col1 + row3);
            colorO = *(srcPtr + col2 + row3);
            colorP = *(srcPtr + col3 + row3);

            if ((colorA == colorD) && (colorB != colorC))
            {
                if ( ((colorA == colorE) && (colorB == colorL)) ||
                    ((colorA == colorC) && (colorA == colorF) && (colorB != colorE) && (colorB == colorJ)) )
                    product = colorA;
                else
                    product = INTERPOLATE4444(colorA, colorB);

                if (((colorA == colorG) && (colorC == colorO)) ||
                    ((colorA == colorB) && (colorA == colorH) && (colorG != colorC) && (colorC == colorM)) )
                    product1 = colorA;
                else
                    product1 = INTERPOLATE4444(colorA, colorC);

                product2 = colorA;
            }
            else if ((colorB == colorC) && (colorA != colorD))
            {
                if (((colorB == colorF) && (colorA == colorH)) ||
                    ((colorB == colorE) && (colorB == colorD) && (colorA != colorF) && (colorA == colorI)) )
                    product = colorB;
                else
                    product = INTERPOLATE4444(colorA, colorB);
 
                if (((colorC == colorH) && (colorA == colorF)) ||
                    ((colorC == colorG) && (colorC == colorD) && (colorA != colorH) && (colorA == colorI)) )
                    product1 = colorC;
                else
                    product1 = INTERPOLATE4444(colorA, colorC);
                product2 = colorB;
            }
            else if ((colorA == colorD) && (colorB == colorC))
            {
                if (colorA == colorB)
                {
                    product = colorA;
                    product1 = colorA;
                    product2 = colorA;
                }
                else
                {
                    s16 r = 0;
                    product1 = INTERPOLATE4444(colorA, colorC);
                    product = INTERPOLATE4444(colorA, colorB);

                    r += GetResult1 (colorA, colorB, colorG, colorE, colorI);
                    r += GetResult2 (colorB, colorA, colorK, colorF, colorJ);
                    r += GetResult2 (colorB, colorA, colorH, colorN, colorM);
                    r += GetResult1 (colorA, colorB, colorL, colorO, colorP);

                    if (r > 0)
                        product2 = colorA;
                    else if (r < 0)
                        product2 = colorB;
                    else
                        product2 = Q_INTERPOLATE4444(colorA, colorB, colorC, colorD);
                }
            }
            else
            {
                product2 = Q_INTERPOLATE4444(colorA, colorB, colorC, colorD);

                if ((colorA == colorC) && (colorA == colorF) && (colorB != colorE) && (colorB == colorJ))
                    product = colorA;
                else if ((colorB == colorE) && (colorB == colorD) && (colorA != colorF) && (colorA == colorI))
                    product = colorB;
                else
                    product = INTERPOLATE4444(colorA, colorB);

                if ((colorA == colorB) && (colorA == colorH) && (colorG != colorC) && (colorC == colorM))
                    product1 = colorA;
                else if ((colorC == colorG) && (colorC == colorD) && (colorA != colorH) && (colorA == colorI))
                    product1 = colorC;
                else
                    product1 = INTERPOLATE4444(colorA, colorC);
            }

			destPtr[0] = colorA;
			destPtr[1] = product;
			destPtr[destWidth] = product1;
			destPtr[destWidth + 1] = product2;

			srcPtr++;
			destPtr += 2;
        }
		destPtr += destWidth;
	}
}

void _2xSaI5551( u16 *srcPtr, u16 *destPtr, u16 width, u16 height, s32 clampS, s32 clampT )
{
	u16 destWidth = width << 1;
	//u16 destHeight = height << 1;

    u32 colorA, colorB, colorC, colorD,
          colorE, colorF, colorG, colorH,
          colorI, colorJ, colorK, colorL,
          colorM, colorN, colorO, colorP;
	u32 product, product1, product2;

	s16 row0, row1, row2, row3;
	s16 col0, col1, col2, col3;

	for (u16 y = 0; y < height; y++)
	{
		if (y > 0)
			row0 = -width;
		else
			row0 = clampT ? 0 : (height - 1) * width;

		row1 = 0;

		if (y < height - 1)
		{
			row2 = width;

			if (y < height - 2) 
				row3 = width << 1;
			else
				row3 = clampT ? width : -y * width;
		}
		else
		{
			row2 = clampT ? 0 : -y * width;
			row3 = clampT ? 0 : (1 - y) * width;
		}

        for (u16 x = 0; x < width; x++)
        {
			if (x > 0)
				col0 = -1;
			else
				col0 = clampS ? 0 : width - 1;

			col1 = 0;

			if (x < width - 1)
			{
				col2 = 1;

				if (x < width - 2) 
					col3 = 2;
				else
					col3 = clampS ? 1 : -x;
			}
			else
			{
				col2 = clampS ? 0 : -x;
				col3 = clampS ? 0 : 1 - x;
			}

//---------------------------------------
// Map of the pixels:                    I|E F|J
//                                       G|A B|K
//                                       H|C D|L
//                                       M|N O|P
            colorI = *(srcPtr + col0 + row0);
            colorE = *(srcPtr + col1 + row0);
            colorF = *(srcPtr + col2 + row0);
            colorJ = *(srcPtr + col3 + row0);

            colorG = *(srcPtr + col0 + row1);
            colorA = *(srcPtr + col1 + row1);
            colorB = *(srcPtr + col2 + row1);
            colorK = *(srcPtr + col3 + row1);

            colorH = *(srcPtr + col0 + row2);
            colorC = *(srcPtr + col1 + row2);
            colorD = *(srcPtr + col2 + row2);
            colorL = *(srcPtr + col3 + row2);

            colorM = *(srcPtr + col0 + row3);
            colorN = *(srcPtr + col1 + row3);
            colorO = *(srcPtr + col2 + row3);
            colorP = *(srcPtr + col3 + row3);

            if ((colorA == colorD) && (colorB != colorC))
            {
                if ( ((colorA == colorE) && (colorB == colorL)) ||
                    ((colorA == colorC) && (colorA == colorF) && (colorB != colorE) && (colorB == colorJ)) )
                    product = colorA;
                else
                    product = INTERPOLATE5551(colorA, colorB);

                if (((colorA == colorG) && (colorC == colorO)) ||
                    ((colorA == colorB) && (colorA == colorH) && (colorG != colorC) && (colorC == colorM)) )
                    product1 = colorA;
                else
                    product1 = INTERPOLATE5551(colorA, colorC);

                product2 = colorA;
            }
            else if ((colorB == colorC) && (colorA != colorD))
            {
                if (((colorB == colorF) && (colorA == colorH)) ||
                    ((colorB == colorE) && (colorB == colorD) && (colorA != colorF) && (colorA == colorI)) )
                    product = colorB;
                else
                    product = INTERPOLATE5551(colorA, colorB);
 
                if (((colorC == colorH) && (colorA == colorF)) ||
                    ((colorC == colorG) && (colorC == colorD) && (colorA != colorH) && (colorA == colorI)) )
                    product1 = colorC;
                else
                    product1 = INTERPOLATE5551(colorA, colorC);
                product2 = colorB;
            }
            else if ((colorA == colorD) && (colorB == colorC))
            {
                if (colorA == colorB)
                {
                    product = colorA;
                    product1 = colorA;
                    product2 = colorA;
                }
                else
                {
                    s16 r = 0;
                    product1 = INTERPOLATE5551(colorA, colorC);
                    product = INTERPOLATE5551(colorA, colorB);

                    r += GetResult1 (colorA, colorB, colorG, colorE, colorI);
                    r += GetResult2 (colorB, colorA, colorK, colorF, colorJ);
                    r += GetResult2 (colorB, colorA, colorH, colorN, colorM);
                    r += GetResult1 (colorA, colorB, colorL, colorO, colorP);

                    if (r > 0)
                        product2 = colorA;
                    else if (r < 0)
                        product2 = colorB;
                    else
                        product2 = Q_INTERPOLATE5551(colorA, colorB, colorC, colorD);
                }
            }
            else
            {
                product2 = Q_INTERPOLATE5551(colorA, colorB, colorC, colorD);

                if ((colorA == colorC) && (colorA == colorF) && (colorB != colorE) && (colorB == colorJ))
                    product = colorA;
                else if ((colorB == colorE) && (colorB == colorD) && (colorA != colorF) && (colorA == colorI))
                    product = colorB;
                else
                    product = INTERPOLATE5551(colorA, colorB);

                if ((colorA == colorB) && (colorA == colorH) && (colorG != colorC) && (colorC == colorM))
                    product1 = colorA;
                else if ((colorC == colorG) && (colorC == colorD) && (colorA != colorH) && (colorA == colorI))
                    product1 = colorC;
                else
                    product1 = INTERPOLATE5551(colorA, colorC);
            }

			destPtr[0] = colorA;
			destPtr[1] = product;
			destPtr[destWidth] = product1;
			destPtr[destWidth + 1] = product2;

			srcPtr++;
			destPtr += 2;
        }
		destPtr += destWidth;
	}
}

void _2xSaI8888( u32 *srcPtr, u32 *destPtr, u16 width, u16 height, s32 clampS, s32 clampT )
{
	u16 destWidth = width << 1;
	//u16 destHeight = height << 1;

    u32 colorA, colorB, colorC, colorD,
          colorE, colorF, colorG, colorH,
          colorI, colorJ, colorK, colorL,
          colorM, colorN, colorO, colorP;
	u32 product, product1, product2;

	s16 row0, row1, row2, row3;
	s16 col0, col1, col2, col3;

	for (u16 y = 0; y < height; y++)
	{
		if (y > 0)
			row0 = -width;
		else
			row0 = clampT ? 0 : (height - 1) * width;

		row1 = 0;

		if (y < height - 1)
		{
			row2 = width;

			if (y < height - 2) 
				row3 = width << 1;
			else
				row3 = clampT ? width : -y * width;
		}
		else
		{
			row2 = clampT ? 0 : -y * width;
			row3 = clampT ? 0 : (1 - y) * width;
		}

        for (u16 x = 0; x < width; x++)
        {
			if (x > 0)
				col0 = -1;
			else
				col0 = clampS ? 0 : width - 1;

			col1 = 0;

			if (x < width - 1)
			{
				col2 = 1;

				if (x < width - 2) 
					col3 = 2;
				else
					col3 = clampS ? 1 : -x;
			}
			else
			{
				col2 = clampS ? 0 : -x;
				col3 = clampS ? 0 : 1 - x;
			}

//---------------------------------------
// Map of the pixels:                    I|E F|J
//                                       G|A B|K
//                                       H|C D|L
//                                       M|N O|P
            colorI = *(srcPtr + col0 + row0);
            colorE = *(srcPtr + col1 + row0);
            colorF = *(srcPtr + col2 + row0);
            colorJ = *(srcPtr + col3 + row0);

            colorG = *(srcPtr + col0 + row1);
            colorA = *(srcPtr + col1 + row1);
            colorB = *(srcPtr + col2 + row1);
            colorK = *(srcPtr + col3 + row1);

            colorH = *(srcPtr + col0 + row2);
            colorC = *(srcPtr + col1 + row2);
            colorD = *(srcPtr + col2 + row2);
            colorL = *(srcPtr + col3 + row2);

            colorM = *(srcPtr + col0 + row3);
            colorN = *(srcPtr + col1 + row3);
            colorO = *(srcPtr + col2 + row3);
            colorP = *(srcPtr + col3 + row3);

            if ((colorA == colorD) && (colorB != colorC))
            {
                if ( ((colorA == colorE) && (colorB == colorL)) ||
                    ((colorA == colorC) && (colorA == colorF) && (colorB != colorE) && (colorB == colorJ)) )
                    product = colorA;
                else
                    product = INTERPOLATE8888(colorA, colorB);

                if (((colorA == colorG) && (colorC == colorO)) ||
                    ((colorA == colorB) && (colorA == colorH) && (colorG != colorC) && (colorC == colorM)) )
                    product1 = colorA;
                else
                    product1 = INTERPOLATE8888(colorA, colorC);

                product2 = colorA;
            }
            else if ((colorB == colorC) && (colorA != colorD))
            {
                if (((colorB == colorF) && (colorA == colorH)) ||
                    ((colorB == colorE) && (colorB == colorD) && (colorA != colorF) && (colorA == colorI)) )
                    product = colorB;
                else
                    product = INTERPOLATE8888(colorA, colorB);
 
                if (((colorC == colorH) && (colorA == colorF)) ||
                    ((colorC == colorG) && (colorC == colorD) && (colorA != colorH) && (colorA == colorI)) )
                    product1 = colorC;
                else
                    product1 = INTERPOLATE8888(colorA, colorC);
                product2 = colorB;
            }
            else if ((colorA == colorD) && (colorB == colorC))
            {
                if (colorA == colorB)
                {
                    product = colorA;
                    product1 = colorA;
                    product2 = colorA;
                }
                else
                {
                    s16 r = 0;
                    product1 = INTERPOLATE8888(colorA, colorC);
                    product = INTERPOLATE8888(colorA, colorB);

                    r += GetResult1 (colorA, colorB, colorG, colorE, colorI);
                    r += GetResult2 (colorB, colorA, colorK, colorF, colorJ);
                    r += GetResult2 (colorB, colorA, colorH, colorN, colorM);
                    r += GetResult1 (colorA, colorB, colorL, colorO, colorP);

                    if (r > 0)
                        product2 = colorA;
                    else if (r < 0)
                        product2 = colorB;
                    else
                        product2 = Q_INTERPOLATE8888(colorA, colorB, colorC, colorD);
                }
            }
            else
            {
                product2 = Q_INTERPOLATE8888(colorA, colorB, colorC, colorD);

                if ((colorA == colorC) && (colorA == colorF) && (colorB != colorE) && (colorB == colorJ))
                    product = colorA;
                else if ((colorB == colorE) && (colorB == colorD) && (colorA != colorF) && (colorA == colorI))
                    product = colorB;
                else
                    product = INTERPOLATE8888(colorA, colorB);

                if ((colorA == colorB) && (colorA == colorH) && (colorG != colorC) && (colorC == colorM))
                    product1 = colorA;
                else if ((colorC == colorG) && (colorC == colorD) && (colorA != colorH) && (colorA == colorI))
                    product1 = colorC;
                else
                    product1 = INTERPOLATE8888(colorA, colorC);
            }

			destPtr[0] = colorA;
			destPtr[1] = product;
			destPtr[destWidth] = product1;
			destPtr[destWidth + 1] = product2;

			srcPtr++;
			destPtr += 2;
        }
		destPtr += destWidth;
	}
}

